package com.headStorm.mavlinkDemo;

import io.dronefleet.mavlink.MavlinkConnection;
import io.dronefleet.mavlink.MavlinkDialect;
import io.dronefleet.mavlink.annotations.MavlinkMessageInfo;
import io.dronefleet.mavlink.common.*;
import io.dronefleet.mavlink.protocol.MavlinkPacket;
import io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadDeserializer;
import io.dronefleet.mavlink.serialization.payload.reflection.ReflectionPayloadSerializer;
import org.checkerframework.checker.units.qual.A;
import org.checkerframework.checker.units.qual.C;
import org.junit.jupiter.api.Test;

import java.io.IOException;
import java.math.BigDecimal;
import java.net.Socket;
import java.nio.ByteBuffer;
import java.text.DecimalFormat;
import java.text.NumberFormat;

public class MavlinkDemo {

    public static void main(String[] args) throws IOException {
        Socket socket = new Socket("127.0.0.1", 5760);
        MavlinkConnection connection = MavlinkConnection.create(
                socket.getInputStream(),
                socket.getOutputStream());
//
//        connection.send1(
//                255, /* systemId */
//                0, /* componentId */
//                Heartbeat.builder()
//                        .type(MavType.MAV_TYPE_GCS)
//                        .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
//                        .systemStatus(MavState.MAV_STATE_UNINIT)
//                        .mavlinkVersion(3)
//                        .build());
//
//        Heartbeat heart = Heartbeat.builder()
//                .type(MavType.MAV_TYPE_GCS)
//                .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
//                .systemStatus(MavState.MAV_STATE_UNINIT)
//                .mavlinkVersion(3)
//                .build();
//        System.out.println(heart.mavlinkVersion());


            //command_long msgId 为76
            byte[] packet2 = Convert.hexStringToByte("FE 21 D4 01 01 4C 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 80 3F 00 00 00 00 00 00 00 00 CB 00 00 00 00 09 3C");
            MavlinkPacket mavlinkPacket2 = MavlinkPacket.fromV1Bytes(packet2);
            System.out.println(mavlinkPacket2);

            //心跳包  mavlink2   fd 09 00 00 80 01 c8 00 00 00 00 00 00 00 04 00 d8 04 03 f1 bf
            byte[] packet3 = Convert.hexStringToByte("fd 09 00 00 80 01 c8 00 00 00 00 00 00 00 04 00 d8 04 03 f1 bf");
            MavlinkPacket mavlinkPacket3 = MavlinkPacket.fromV1Bytes(packet3);
            System.out.println(mavlinkPacket3);
            System.out.println(mavlinkPacket3.isMavlink2());

        }


    @Test
    public void test() {
        //心跳包 msgId为0
        byte[] packet = Convert.hexStringToByte("fd 09 00 00 80 01 c8 00 00 00 00 00 00 00 04 00 d8 04 03 f1 bf");
        MavlinkPacket mavlinkPacket = MavlinkPacket.fromV1Bytes(packet);
        MavlinkDialect dialect = new CommonDialect();
        ReflectionPayloadDeserializer deserializer = new ReflectionPayloadDeserializer();
        if (dialect.supports(mavlinkPacket.getMessageId())) {
            Class<?> messageType = dialect.resolve(mavlinkPacket.getMessageId());
            MavlinkMessageInfo messageInfo = (MavlinkMessageInfo) messageType.getAnnotation(MavlinkMessageInfo.class);
            if (mavlinkPacket.validateCrc(messageInfo.crc())) {
                //Object payload = this.deserializer.deserialize(mavlinkPacket.getPayload(), messageType);
                Object payload = deserializer.deserialize(mavlinkPacket.getPayload(), messageType);
                if (payload instanceof Heartbeat) {
                    Heartbeat heartbeat = (Heartbeat) payload;
//                    if (this.dialects.containsKey(heartbeat.autopilot().entry())) {
//                        this.systemDialects.put(mavlinkPacket.getSystemId(), this.dialects.get(heartbeat.autopilot().entry()));
//                    }
                }
                if (mavlinkPacket.isMavlink2()) {

                    Mavlink2Message var11 = new Mavlink2Message(mavlinkPacket, payload);
                }

                MavlinkMessage var10 = new MavlinkMessage(mavlinkPacket, payload);
            }
            Object payload = deserializer.deserialize(mavlinkPacket.getPayload(), messageType);
            MavlinkMessage<Heartbeat> heartbeatMavlinkMessage = new MavlinkMessage<Heartbeat>(mavlinkPacket,(Heartbeat)payload);
        }
    }

    /**
     * 测试commandLong命令,相机触发。
     */
    @Test
    public void  testCommandLongTriggerPhoto(){
        byte[] packet = Convert.hexStringToByte("fe 21 00 ff be 4c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 80 3f 00 00 00 00 00 00 00 00 cb 00 00 00 00 a5 24");
        MavlinkPacket mavlinkPacket = MavlinkPacket.fromV1Bytes(packet);
        MavlinkDialect dialect = new CommonDialect();
        Class<?> messageType = dialect.resolve(mavlinkPacket.getMessageId());
        ReflectionPayloadDeserializer deserializer = new ReflectionPayloadDeserializer();
        Object payload = deserializer.deserialize(mavlinkPacket.getPayload(), messageType);
        MavlinkMessage message = new MavlinkMessage(mavlinkPacket,payload);
        MavlinkMessage<CommandLong> commandLongMavlinkMessage = (MavlinkMessage<CommandLong>)message;
        System.out.println(commandLongMavlinkMessage.getPayload());
    }

    /**
     * 测试commandLong命令,相机触发。
     */
    @Test
    public void testCommandLongBackToMiddle(){
        byte[] packet = Convert.hexStringToByte("fe 21 00 ff be 4c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 a0 40 00 00 00 00 00 00 00 00 cb 00 00 00 00 9e ba");
        MavlinkPacket mavlinkPacket = MavlinkPacket.fromV1Bytes(packet);
        MavlinkDialect dialect = new CommonDialect();
        Class<?> messageType = dialect.resolve(mavlinkPacket.getMessageId());
        ReflectionPayloadDeserializer deserializer = new ReflectionPayloadDeserializer();
        Object payload = deserializer.deserialize(mavlinkPacket.getPayload(), messageType);
        MavlinkMessage message = new MavlinkMessage(mavlinkPacket,payload);
        System.out.println(message);
        MavlinkMessage<CommandLong> commandLongMavlinkMessage = (MavlinkMessage<CommandLong>)message;
        System.out.println(commandLongMavlinkMessage.getPayload());
    }

    public MavlinkMessage<CommandLong> parse(String str){
        //byte[] packet = Convert.hexStringToByte("fe 21 00 ff be 4c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 a0 40 00 00 00 00 00 00 00 00 cb 00 00 00 00 9e ba");
        byte[] packet = Convert.hexStringToByte(str);
        MavlinkPacket mavlinkPacket = MavlinkPacket.fromV2Bytes(packet);
        MavlinkDialect dialect = new CommonDialect();
        Class<?> messageType = dialect.resolve(mavlinkPacket.getMessageId());
        ReflectionPayloadDeserializer deserializer = new ReflectionPayloadDeserializer();
        Object payload = deserializer.deserialize(mavlinkPacket.getPayload(), messageType);
        MavlinkMessage message = new MavlinkMessage(mavlinkPacket,payload);
        System.out.println(message);
        MavlinkMessage<CommandLong> commandLongMavlinkMessage = (MavlinkMessage<CommandLong>)message;
        return commandLongMavlinkMessage;
    }

    private int sequence = 0;

    /**
     * 生成心跳包byte字节数组
     */
    @Test
    public void testProduceHeartBeatBytes(){
        int systemId = 255;
        int componentId = 0;
        Heartbeat payload = Heartbeat.builder()
                .type(MavType.MAV_TYPE_GCS)
                .autopilot(MavAutopilot.MAV_AUTOPILOT_INVALID)
                .systemStatus(MavState.MAV_STATE_UNINIT)
                .mavlinkVersion(3)
                .build();

        MavlinkMessageInfo messageInfo = (MavlinkMessageInfo)payload.getClass().getAnnotation(MavlinkMessageInfo.class);
        ReflectionPayloadSerializer serializer = new ReflectionPayloadSerializer();
        byte[] serializedPayload = serializer.serialize(payload);
        MavlinkPacket unsignedMavlink2Packet = MavlinkPacket.createUnsignedMavlink2Packet(this.sequence++, systemId, componentId, messageInfo.id(), messageInfo.crc(), serializedPayload);
        String string = Convert.bytesToHexString(unsignedMavlink2Packet.getRawBytes(), true);
        System.out.println(string);
    }


    //------------------------------------------------------------------------------
    /**
     * 将合法的字节数组转换成对应的message对象
     * @param packet
     * @param <T>
     * @return
     */
    public <T> MavlinkMessage<T> bytesToMessageObject(byte [] packet){
        //判断协议版本
        int stx = (packet[0] & 0xff);
        MavlinkPacket mavlinkPacket = null;
        if(stx == 0xfe){
             //mavlink1
             mavlinkPacket = MavlinkPacket.fromV1Bytes(packet);
        }else if(stx == 0xfd){
             //mavlink2
             mavlinkPacket = MavlinkPacket.fromV2Bytes(packet);
        }
        //指定解析的方言
        MavlinkDialect dialect = new CommonDialect();
        //获取消息类型，并反序列化payload
        Class<?> messageType = dialect.resolve(mavlinkPacket.getMessageId());
        ReflectionPayloadDeserializer deserializer = new ReflectionPayloadDeserializer();
        Object payload = deserializer.deserialize(mavlinkPacket.getPayload(), messageType);
        MavlinkMessage message = new MavlinkMessage(mavlinkPacket,payload);
        MavlinkMessage<T> result = (MavlinkMessage<T>)message;
        return result;
    }

    //解析心跳
    @Test
    public void parseHeart(){
        String h1 = "FE 09 66 02 01 00 00 00 01 00 02 0C 41 03 03 D0 9F";
        String h2 = "fd 09 00 00 80 01 c8 00 00 00 00 00 00 00 04 00 d8 04 03 f1 bf";

        byte[] bs1 = Convert.hexStringToByte(h1);
        MavlinkMessage<Heartbeat> message1 = bytesToMessageObject(bs1);
        System.out.println(message1.getPayload());

        byte[] bs2= Convert.hexStringToByte(h2);
        MavlinkMessage<Heartbeat> message2 = bytesToMessageObject(bs1);
        System.out.println(message2.getPayload());
    }

    //解析commandLong
    @Test
    public void parseCommandLong(){
        String n2 ="FD 01 00 00 BD 01 01 2A 00 00 00 27 54 FD 14 00 00 BE 01 01 1F 00 00 E1 6B BD 00 3A 2A 7C 3F 4B B8 7A 3B 1B 48 54 BC 39 0A 30 BE 7F 4F";
        MavlinkPacket mavlinkPacket = MavlinkPacket.fromV2Bytes(Convert.hexStringToByte(n2));
        System.out.println(mavlinkPacket);
    }


    @Test
    public void   produce(Object payload,int systemId,int componentId){
        MavlinkMessageInfo messageInfo = (MavlinkMessageInfo)payload.getClass().getAnnotation(MavlinkMessageInfo.class);
        ReflectionPayloadSerializer serializer = new ReflectionPayloadSerializer();
        byte[] serializedPayload = serializer.serialize(payload);



        MavlinkPacket mavlink1Packet = MavlinkPacket.createMavlink1Packet(30, systemId, componentId, messageInfo.id(), messageInfo.crc(), serializedPayload);
        String string1 = Convert.bytesToHexString(mavlink1Packet.getRawBytes(), true);

        int len = serializedPayload.length;
        for (int i = serializedPayload.length-1 ;i >0;i--){
            if(serializedPayload[i]==0){
                len--;
            }else{
                break;
            }
        }
        byte [] serializedPayload2 = new byte [len];
        System.arraycopy(serializedPayload,0,serializedPayload2,0,len);
        MavlinkPacket unsignedMavlink2Packet = MavlinkPacket.createUnsignedMavlink2Packet(188, systemId, componentId, messageInfo.id(), messageInfo.crc(), serializedPayload2);

        String string2 = Convert.bytesToHexString(unsignedMavlink2Packet.getRawBytes(), true);


        System.out.println("mavlink1:"+string1);
        System.out.println("mavlink2:"+string2);
    }

    @Test
    public void produceCommandLong(){
        int systemId = 255;
        int componentId = 0;
        //回中
        CommandLong payload = CommandLong.builder()
                .command(MavCmd.MAV_CMD_DO_DIGICAM_CONTROL)
                .targetComponent(1)
                .targetSystem(1)
                .param1(0)
                .param2(0)
                .param3(0)
                .param4(0)
                .param5(5)
                .param6(0)
                .param7(0)
                .build();
        produce(payload,systemId,componentId);

        //转动
        CommandLong payload2 = CommandLong.builder()
                .command(MavCmd.MAV_CMD_DO_MOUNT_CONTROL)
                .targetComponent(1)
                .targetSystem(1)
                .param1(8f)
                .param2(0f)
                .param3(8f)
                .param4(0)
                .param5(1f)
                .param6(0)
                .param7(0)
                .build();
        produce(payload2,systemId,componentId);

        //LOGI_ON
//        CommandLong payload3 = CommandLong.builder()
//                                .command(MavCmd.MAV_CMD_MY1).param1(1).param2(0).build();
//        produce(payload3,systemId,componentId);
    }

    /**
     * 生成回中命令
     */
    @Test
    public void produceCommandLongBackToMiddle(){
        int systemId = 255;
        int componentId = 0;
        CommandLong payload = CommandLong.builder()
                .command(MavCmd.MAV_CMD_DO_DIGICAM_CONTROL)
                .targetComponent(1)
                .targetSystem(1)
                .param1(0)
                .param2(0)
                .param3(0)
                .param4(0)
                .param5(5)
                .param6(0)
                .param7(0)
                .build();
        produce(payload,systemId,componentId);
    }

    //------------------------------------------------------------------------------

    @Test
    public void test6() {
        String strRuimi = "fd 20 00 00 bc ff 00 4c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 a0 40 00 00 00 00 00 00 00 00 cb  00 01 01 00 a2 01";
        String strMy =     "FD 20 00 00 BC FF 00 4C 00 00 00 00 00 41 00 00 00 00 00 00 00 41 00 00 00 00 00 00 80 3F 00 00 00 00 00 00 00 00 CD 00 01 01 3D 20";

        //        MavlinkPacket mavlinkPacket = MavlinkPacket.fromV2Bytes(Convert.hexStringToByte(strRuimi));
        //   System.out.println(mavlinkPacket.getPayload().length);

        MavlinkMessage<CommandLong> messageRuimi = bytesToMessageObject(Convert.hexStringToByte(strRuimi));
        MavlinkMessage<CommandLong> messageMy = bytesToMessageObject(Convert.hexStringToByte(strMy));
        System.out.println(messageRuimi.getPayload());
        System.out.println(messageMy.getPayload());
    }

    @Test
    public void test7(){
        String a = "fe 21 00 ff be 4c 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 a0 40 00 00 00 00 00 00 00 00 cb 00 00 00 00 9e ba";
        a.split(" ");
        System.out.println(a.length());
    }

    @Test
    public void test8(){

        String vmount1 = "FE 14 12 01 01 B4 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 07 9E";
        String vmount2 = "FD 14 00 00 56 01 01 B4 00 00 C1 44 22 00 00 00 00 00 14 AE A7 BF 8F C2 75 3D B8 1E 85 BE F0 67";
        String vmount3 = "FE 14 FD 01 01 B4 00 00 00 00 00 00 00 00 01 01 00 00 02 02 00 00 03 00 00 00 AD 14";
        String vmount4 = "FD 14 00 00 FF 01 01 B4 00 00 B5 24 4C 00 00 00 00 00 70 3D 4A 40 66 E6 92 42 85 EB 98 42 4B 1B";

        byte[] payload = MavlinkPacket.fromV1Bytes(Convert.hexStringToByte(vmount4)).getPayload();
        //System.out.println(getFloat(Convert.hexStringToByte("8FC2A941"))); //21.22
        System.out.println(getFloat(Convert.hexStringToByte("70 3D 4A 40")));

        byte[] b1 = new byte[4];
        byte[] b2 = new byte[4];
        byte[] b3 = new byte[4];

        System.arraycopy(payload,8,b1,0,4);
        System.arraycopy(payload,12,b1,0,4);
        System.arraycopy(payload,16,b2,0,4);
       // System.out.println(getFloat(b1));
        System.out.println(getFloat(b2));
       // System.out.println(getFloat(b3));

        byte[] payload2= MavlinkPacket.fromV2Bytes(Convert.hexStringToByte(vmount2)).getPayload();
        System.out.println(payload2.length);
    }

    /**
     * 字节转换为浮点
     * @param b 字节（至少4个字节）
     * @param b 开始位置
     * @return
     */
    public static float getFloat(byte[] b) {
        int accum = 0;
        accum = accum|(b[0] & 0xff) << 0;
        accum = accum|(b[1] & 0xff) << 8;
        accum = accum|(b[2] & 0xff) << 16;
        accum = accum|(b[3] & 0xff) << 24;
        System.out.println(accum);
        return Float.intBitsToFloat(accum);
    }

     @Test
     public void test4(){
         System.out.println(Math.sqrt(Math.pow(2.3,2)+Math.pow(2,2)));
     }

     @Test
     public void test5(){
//         float f = 0.000000001f;
//         DecimalFormat format = (DecimalFormat) NumberFormat.getInstance();
//         format.applyPattern("0.0000001");
//         System.out.println("科学计数法：" + f);
//         System.out.println("原数字du显示:" + format.format(f));

         //String sjiachun = "12345E-10";
         String sjiachun = "0.0000012345";
         BigDecimal db = new BigDecimal(sjiachun);
         String ii = db.toPlainString();
         System.out.println(ii);

         String str = String.format("%.2f", Double.parseDouble(ii));
         System.out.println(str);//30.67
     }

}